#include "StdAfx.h"
#include "DXUT.h"
#include "SphereTree.h"

SphereTreeIndex::SphereTreeIndex(int iLevel_, DWORD dwIndex_)
: iLevel(iLevel_)
, dwIndex(dwIndex_)
{
}

CSphereTreeNode::CSphereTreeNode(int iLevel)
: m_iLevel(iLevel)
, m_pLeft(NULL)
, m_pRight(NULL)
, m_pSphereMesh(NULL)
, m_fSphereRadius(0)
{

}

CSphereTreeNode::~CSphereTreeNode()
{
	delete m_pLeft;
	delete m_pRight;
}

void CSphereTreeNode::BuildTree(const std::vector<VectorR>& vVertices, const std::vector<CTriangle>& vTriangles, const std::vector<int> vTriangleIndices, int iMaxLevel, DWORD dwIndex)
{
	if (vTriangleIndices.size() == 0)
		return;

	m_dwIndex = dwIndex;

	// compute center
	VectorR center;
	for (unsigned int i = 0; i < 3; ++i)
		center[i] = 0.0;

	for (unsigned int i = 0; i < vTriangleIndices.size(); ++i)
	{
		int iIndex = vTriangleIndices[i];
		const CTriangle& triangle = vTriangles[iIndex];
		center += vVertices[triangle.index[0]];
		center += vVertices[triangle.index[1]];
		center += vVertices[triangle.index[2]];
	}
	center /= 3.0;
	if (vTriangleIndices.size())
	{
		center /= vTriangleIndices.size();
	}

	// compute eigenvector
	// compute covariance matrix
	Matrix3R covarianceMatrix;
	for (unsigned int i = 0; i < 3; ++i)
	{
		for (unsigned int j = 0; j < 3; ++j)
		{
			covarianceMatrix(i, j) = 0.0;
		}
	}

	for (unsigned int i = 0; i < vTriangleIndices.size(); ++i)
	{
		int iIndex = vTriangleIndices[i];
		const CTriangle& triangle = vTriangles[iIndex];

		for (unsigned int j = 0; j < 3; ++j)
		{
			for (unsigned int k = 0; k < 3; ++k)
			{
				VectorR p = vVertices[triangle.index[0]] - center;
				VectorR q = vVertices[triangle.index[1]] - center;
				VectorR r = vVertices[triangle.index[2]] - center;

				covarianceMatrix(j, k) += p(j) * p(k);
				covarianceMatrix(j, k) += q(j) * q(k);
				covarianceMatrix(j, k) += r(j) * r(k);
			}
		}
	}
	covarianceMatrix /= 3.0;
	if (vTriangleIndices.size())
	{
		covarianceMatrix /= vTriangleIndices.size();
	}

	Eigen::SelfAdjointEigenSolver<Matrix3R> eigensolver(covarianceMatrix);

	VectorR eigenVectors[3];
	VectorR eigenValues = eigensolver.eigenvalues();
	for (unsigned int i = 0; i < 3; ++i)
	{
		eigenVectors[i] = eigensolver.eigenvectors().col(i);
		eigenVectors[i].normalize();
	}

	// find medial axis & sphere center
	VectorR medialAxis = eigenVectors[0];
	DATA_TYPE maxEigenValue = eigenValues[0];
	for (unsigned int i = 1; i < 3; ++i)
	{
		if (eigenValues[i] > maxEigenValue)
		{
			maxEigenValue = eigenValues[i];
			medialAxis = eigenVectors[i];
		}
	}

	// generate sphere mesh
	// TODO: Bounding sphere computation
	std::vector<D3DXVECTOR3> vDXVertices;
	for (unsigned int i = 0; i < vTriangleIndices.size(); ++i)
	{
		int iIndex = vTriangleIndices[i];
		const CTriangle& triangle = vTriangles[iIndex];

		for (unsigned int j = 0; j < 3; ++j)
		{
			const VectorR& vertex = vVertices[triangle.index[j]];
			vDXVertices.push_back(D3DXVECTOR3((float)vertex[0], (float)vertex[1], (float)vertex[2]));
		}
	}

	m_vSphereCenter.x = (float)center[0];
	m_vSphereCenter.y = (float)center[1];
	m_vSphereCenter.z = (float)center[2];

	DATA_TYPE fMaxSqDist = 0;
	for (unsigned int i = 0; i < vTriangleIndices.size(); ++i)
	{
		int iIndex = vTriangleIndices[i];
		const CTriangle& triangle = vTriangles[iIndex];

		for (unsigned int j = 0; j < 3; ++j)
		{
			const VectorR& vertex = vVertices[triangle.index[j]];
			
			DATA_TYPE fSqDist = (vertex - center).squaredNorm();
			if (fSqDist > fMaxSqDist)
				fMaxSqDist = fSqDist;
		}
	}
	m_fSphereRadius = (float)sqrt(fMaxSqDist);

	D3DXCreateSphere(DXUTGetD3D9Device(), m_fSphereRadius, 8, 8, &m_pSphereMesh, NULL);

	if (m_iLevel == iMaxLevel)
		return;

	// divide triangles to two group
	std::vector<int> vTriangleIndicesLeft;
	std::vector<int> vTriangleIndicesRight;

	for (unsigned int i = 0; i < vTriangleIndices.size(); ++i)
	{
		int iIndex = vTriangleIndices[i];
		const CTriangle& triangle = vTriangles[iIndex];

		VectorR vTriangleCenter = ( vVertices[triangle.index[0]] + vVertices[triangle.index[1]] + vVertices[triangle.index[2]] ) / 3;
		VectorR v = vTriangleCenter - center;
		if ( v.dot(medialAxis) >= 0 )
		{
			vTriangleIndicesLeft.push_back(iIndex);
		}
		else
		{
			vTriangleIndicesRight.push_back(iIndex);
		}
	}

	// generate left & right node if (# of triangle > 0)
	if (vTriangleIndicesLeft.size() > 0)
	{
		m_pLeft = new CSphereTreeNode(m_iLevel + 1);
		m_pLeft->BuildTree(vVertices, vTriangles, vTriangleIndicesLeft, iMaxLevel, dwIndex);
	}
	if (vTriangleIndicesRight.size() > 0)
	{
		m_pRight = new CSphereTreeNode(m_iLevel + 1);
		m_pRight->BuildTree(vVertices, vTriangles, vTriangleIndicesRight, iMaxLevel, (dwIndex | (1 << (m_iLevel + 1))));
	}
}

void CSphereTreeNode::Render(int iLevel, const D3DXVECTOR3& vTranslate)
{
	if (m_iLevel == iLevel)
	{
		DoRender(vTranslate);
	}
	else
	{
		if (m_pLeft)
			m_pLeft->Render(iLevel, vTranslate);

		if (m_pRight)
			m_pRight->Render(iLevel, vTranslate);
	}
}

void CSphereTreeNode::DoRender(const D3DXVECTOR3& vTranslate)
{
	if (m_pSphereMesh)
	{
		D3DXMATRIX matWorld;
		D3DXMatrixTranslation(&matWorld, m_vSphereCenter.x + vTranslate.x, m_vSphereCenter.y + vTranslate.y, m_vSphereCenter.z + vTranslate.z);
		DXUTGetD3D9Device()->SetTransform( D3DTS_WORLD, &matWorld );
		m_pSphereMesh->DrawSubset(0);
	}
}

void CSphereTreeNode::InitD3DBuffer()
{
	SAFE_RELEASE(m_pSphereMesh);

	D3DXCreateSphere(DXUTGetD3D9Device(), m_fSphereRadius, 8, 8, &m_pSphereMesh, NULL);

	if (m_pLeft)
		m_pLeft->InitD3DBuffer();

	if (m_pRight)
		m_pRight->InitD3DBuffer();
}

void CSphereTreeNode::Destroy()
{
	SAFE_RELEASE(m_pSphereMesh);

	if (m_pLeft)
		m_pLeft->Destroy();

	if (m_pRight)
		m_pRight->Destroy();
}

int CSphereTreeNode::CountNodes(int iLevel) const
{
	if (m_iLevel == iLevel)
		return 1;
	else
	{
		int iCount = 0;
		if (m_pLeft)
			iCount += m_pLeft->CountNodes(iLevel);
		if (m_pRight)
			iCount += m_pRight->CountNodes(iLevel);
		return iCount;
	}
}

void CSphereTreeNode::Translate(const D3DXVECTOR3& vTranslate)
{
	m_vSphereCenter += vTranslate;

	if (m_pLeft)
		m_pLeft->Translate(vTranslate);
	if (m_pRight)
		m_pRight->Translate(vTranslate);
}

const CSphereTreeNode* CSphereTreeNode::GetNode(const SphereTreeIndex& index) const
{
	if (index.iLevel == m_iLevel)
	{
		assert(index.dwIndex == m_dwIndex);
		return this;
	}
	DWORD dwMask = (1 << (m_iLevel + 1));
	
	CSphereTreeNode* pNextChild = (dwMask & index.dwIndex) ? m_pRight : m_pLeft;
	if (pNextChild)
		return pNextChild->GetNode(index);
	return NULL;
}

////////////////////////////////////////////////////////////////////////////////

CSphereTree::CSphereTree(void)
: m_pRoot(NULL)
{
}


CSphereTree::~CSphereTree(void)
{
	delete m_pRoot;
}

void CSphereTree::BuildTree(const std::vector<VectorR>& vVertices, const std::vector<CTriangle>& vTriangles, int iMaxLevel)
{
	delete m_pRoot;
	m_pRoot = new CSphereTreeNode(0);

	std::vector<int> vTriangleIndices;
	for (unsigned int i = 0; i < vTriangles.size(); ++i)
		vTriangleIndices.push_back(i);

	m_pRoot->BuildTree(vVertices, vTriangles, vTriangleIndices, iMaxLevel);
}

void CSphereTree::Render(int iLevel, const D3DXVECTOR3& vTranslate)
{
	if (m_pRoot)
		m_pRoot->Render(iLevel, vTranslate);
}

void CSphereTree::InitD3DBuffer()
{
	if (m_pRoot)
		m_pRoot->InitD3DBuffer();
}

void CSphereTree::Destroy()
{
	if (m_pRoot)
		m_pRoot->Destroy();
}

int CSphereTree::CountNodes(int iLevel) const
{
	if (m_pRoot)
		return m_pRoot->CountNodes(iLevel);

	return 0;
}

void CSphereTree::Translate(const D3DXVECTOR3& vTranslate)
{
	if (m_pRoot)
		m_pRoot->Translate(vTranslate);
}

const CSphereTreeNode* CSphereTree::GetNode(const SphereTreeIndex& index) const
{
	if (m_pRoot)
		return m_pRoot->GetNode(index);

	return NULL;
}